// ///////////////////////////// MIT License //////////////////////////////////// //
//                                                                                //
// Copyright (c) 2010 David Zsolt Manrique                                        //
//                    david.zsolt.manrique@gmail.com                              //
//                                                                                //
// Permission is hereby granted, free of charge, to any person obtaining a copy   //
// of this software and associated documentation files (the "Software"), to deal  //
// in the Software without restriction, including without limitation the rights   //
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell      //
// copies of the Software, and to permit persons to whom the Software is          //
// furnished to do so, subject to the following conditions:                       //
//                                                                                //
// The above copyright notice and this permission notice shall be included in     //
// all copies or substantial portions of the Software.                            //
//                                                                                //
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR     //
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,       //
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE    //
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER         //
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,  //
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN      //
// THE SOFTWARE.                                                                  //
//                                                                                //
// ////////////////////////////////////////////////////////////////////////////// //

//!
//!  Description: recommended fitting algorithm
//!
//!  Levenberg-Marquardt minimization and curve fitting 
//!  Ref:
//!    Is Levenberg-Marquardt the Most Efficient Optimization Algorithm for
//!    Implementing Bundle Adjustment?
//!    Manolis I.A. Lourakis and Antonis A. Argyros
//!    Institute of Computer Science, Foundation for Research and Technology - Hellas
//!    Vassilika Vouton, P.O. Box 1385, GR 711 10, Heraklion, Crete, GREECE lourakis,argyros@ics.forth.gr


#ifndef FF_FIT_H
#define FF_FIT_H

#include "structure_io.h"
#include "control.h"

struct fit_levenberg_marquardt
{
	double tau;
	double e1; 
	double e2; 
	int kmax;
	fit_levenberg_marquardt() {}

	int n;
	int m;
	int k;
	double deviation;
	void operator () (Eigen::VectorXd & p,
		 const std::vector< position_t > & xi_vector,
		 const std::vector<double> & yi_vector,
		 potential_fit_list_t & pl)
	{
		n = yi_vector.size();
		m = p.rows();
		k = 0;
		double nu = 2.0;
		double mu = 0.0;
		double rho = 0.0;
		Eigen::MatrixXd J(m,n);
		Eigen::MatrixXd A(m,m);
		Eigen::MatrixXd I = Eigen::MatrixXd::Identity(m,m);
		Eigen::VectorXd p_new(m);
		Eigen::VectorXd yi = Eigen::VectorXd::Map(&yi_vector[0],yi_vector.size());; 
		Eigen::VectorXd ep(n);
		Eigen::VectorXd g(m);
		Eigen::VectorXd fi(n);
		Eigen::VectorXd fi_new(n);
		Eigen::VectorXd dp(m);
		Eigen::VectorXd Jcol(m);
		bool stop = false;

		for(int i = 0; i < n; i++)
		{
			pl.peval(fi(i),Jcol,p,xi_vector[i]);
			J.col(i) = Jcol;
		}
		A = J*J.transpose();


		ep = yi-fi;
		g = J*ep;
	
		stop = (g.lpNorm<Eigen::Infinity>() <= e1);
		mu = tau * A.diagonal().maxCoeff();

		while (!stop && (k < kmax))
		{
			k++;
			do
			{
				(A+mu*I).lu().solve(g, &dp); 

				if(dp.norm() <= e2* p.norm() )
					stop = true;
				else
				{
					p_new = p + dp;

					for(int i = 0; i < n; i++)
						pl.peval(fi_new(i),Jcol,p_new,xi_vector[i]);
		

					rho = (ep.squaredNorm()-(yi-fi_new).squaredNorm())/dp.dot(mu*dp+g);
					if(rho > 0)
					{
						p.swap(p_new);   //swapping with local variable ezjo?

						for(int i = 0; i < n; i++)
						{
							pl.peval(fi(i),Jcol,p,xi_vector[i]);
							J.col(i) = Jcol;
						}
						A = J*J.transpose();

						fi.swap(fi_new);
				
						ep = yi-fi;
						g = J*ep;
						stop = (g.lpNorm<Eigen::Infinity>() <= e1);
						mu = mu * std::max(1.0/3.0,1.0-(2.0*rho-1.0)*(2.0*rho-1.0)*(2.0*rho-1.0));
						nu = 2.0;
					}
					else
					{
						mu = mu*nu;
						nu = 2*nu;
					}
				}
			} while (!((rho > 0) || (stop)));
		}
		
		//deviation
		deviation = 0.0;
		for(int i = 0; i < n; i++)
		{
			double pot = 0.0;
			pl.peval(pot,Jcol,p,xi_vector[i]);
			deviation = deviation < std::abs(pot-yi(i)) ? std::abs(pot-yi(i)) :deviation;
		}
	}


};

bool operator >>= (const std::vector<std::string> & input_lines,fit_levenberg_marquardt & get);
std::ostream & operator << (std::ostream & os,const fit_levenberg_marquardt & get);
const std::vector<std::string> & operator >> (const std::vector<std::string> & input_lines,varname_t & get);
const std::vector<std::string> & operator >> (const std::vector<std::string> & input_lines,xyzfilelist_t & get);


/*
		std::vector<std::string> block;
		io::parse_block("<velocity-verlet>","<\\velocity-verlet>",input_lines,block);
		
		for(std::vector<std::string>::iterator line = block.begin(); line != block.end(); line++)
		if(io::match("t0 %1",*line,&t0)) ;
		else if (io::match("t1 %1",*line,&t1)) ;
		else if (io::match("dt %1",*line,&dt)) ;
		else if (io::match("DT %1",*line,&DT)) ;
		else throw std::runtime_error("Syntax error: "+*line);
*



/*
struct steepest_descent
{
	const double kappa;
	const double prec; 
	const int kmax;
	steepest_descent(const double kappa = 1.0e-3,
		 const double prec = 1.0e-5,
		 const int kmax = 1000) : kappa(kappa),prec(prec),kmax(kmax)
	{}

	int n;
	int k;
	void operator () (std::vector<Eigen::Vector3d> & r,
		 const Eigen::VectorXd & p,
		 const full_potential & full)
	{
		k = 0;
		n = r.size();
		std::vector<Eigen::Vector3d>  f(n);
		std::vector<Eigen::Vector3d>  rn = r;

		bool stop = false;

		while (!stop && (k < kmax))
		{
			k++;

			full.force(f,p,r);
			for(int i = 0; i < n; i++)
				rn[i] = r[i] + kappa*f[i];

			double d = 0.0;
			for(int i = 0; i < n; i++)
			{
				double max_diff = (r[i]-rn[i]).cwise().abs().maxCoeff();
				if( max_diff > d ) d = max_diff;
			}
			if(d < prec ) stop = true;
			rn.swap(r);
		}
				
	}

	void write(std::ostream & os)
	{
		os << "Number of atoms : " << n << std::endl;
		os << "Number of iteration : " << k << std::endl;
		if(k >= kmax) 
			os << " >>> Maximum "<< kmax << " iteration achieved! <<< " << std::endl; 
	}


};


struct velocity_verlet
{
	const double DT;
	const double dt;
	const double t0;
	const double t1;
	velocity_verlet(const double t0,const double t1,
			const double dt,const double DT) : t0(t0),t1(t1),dt(dt),DT(DT)
	{}

	void operator () (std::ostream & os,
			//! TODO unit cell input?
			const std::vector<Eigen::Vector3d> & r0,
			const std::vector<Eigen::Vector3d> & v0,
			const std::vector<double> & mass,
			const std::vector<std::string> & label,
			const std::vector<bool> & constr, //! TODO more advanced constrain
			const Eigen::VectorXd & p, const full_potential & full)
	{
		size_t n = r0.size();
		std::vector<Eigen::Vector3d>  f(n);
		std::vector<Eigen::Vector3d>  an(n),a(n);
		std::vector<Eigen::Vector3d>  rn(n),r(n);
		std::vector<Eigen::Vector3d>  vn(n),v(n);

		r = r0;
		v = v0;

		full.force(f,p,r); //! at t0  TODO time dependent external potential
		for(std::vector<Eigen::Vector3d>::size_type i = 0; i < n; i++)
		a[i]=f[i]/mass[i];

		double t = t0;
		double T = 0.0;
		
		while(t <= t1)
		{
			for(std::vector<Eigen::Vector3d>::size_type i = 0; i < n; i++)
			if(constr[i])
			rn[i] = r[i] +dt*v[i] + 0.5*dt*dt*a[i];
			
			full.force(f,p,rn); // at t+dt
			for(std::vector<Eigen::Vector3d>::size_type i = 0; i < n; i++)
			an[i]=f[i]/mass[i];
			for(std::vector<Eigen::Vector3d>::size_type i = 0; i < n; i++)
			if(constr[i])
			vn[i] = v[i] + 0.5*dt*(a[i]+an[i]);
			t+=dt;
			rn.swap(r);
			an.swap(a);
			vn.swap(v);
			
			T+=dt;
			if(T > DT)  //!TODO t=t0 print
			{
				T = 0.0;
				double K = 0.0; //! kinetic energy
				for(std::vector<Eigen::Vector3d>::size_type i = 0; i < n; i++)
				K += 0.5*mass[i]*v[i].dot(v[i]);
				double U = 0.0; //! potential energy
				full.potential(U,p,r); //! at t
				
				os << n << ' ';
				os << std::endl;
				os << "0 0 0  0 0 0  0 0 0 " << "time=" << t << ' ';
				os << "kinetic=" << K << ' ';
				os << "potential=" << U << ' ';
				os << "total=" << K+U << ' ';
				os << std::endl;
				for(std::vector<Eigen::Vector3d>::size_type i = 0; i < n; i++)
				{
					os << label[i] << "  ";
					os << r[i].x() << ' ';
					os << r[i].y() << ' ';
					os << r[i].z() << ' ';

					os << v[i].x() << ' ';
					os << v[i].y() << ' ';
					os << v[i].z() << ' ';

					os << f[i].x() << ' ';
					os << f[i].y() << ' ';
					os << f[i].z() << ' ';
					os << std::endl;
				}
			}
		}
	}


	void write(std::ostream & os)
	{
		os << "Number of atoms : " << n << std::endl;
		os << "Number of iteration : " << k << std::endl;
		if(k >= kmax) 
			os << " >>> Maximum "<< kmax << " iteration achieved! <<< " << std::endl; 
	}


};

*/

#endif //FF_FIT_H

